#include "kinematic.h"
#include <iostream>
#include <cmath>

Eigen::Matrix4d robotBase_to_leftArmBase, robotBase_to_rightArmBase, leftArmBase_to_elbow, leftElbow_to_wrist;
Eigen::Matrix3d left_elbow_correct, right_elbow_correct;

Eigen::VectorXd left_last_joints(7), right_last_joints(7);

void Matrix_Init()
{
    robotBase_to_leftArmBase << 0.96592583,0,-0.25881905,-230.37330957,
                                0.        ,1,0.        ,  0.        ,
                                0.25881905,0,0.96592583,-61.72834225,
                                0.        ,0,0.        ,  1;

    robotBase_to_rightArmBase << 0.96592583,0,0.25881905,230.37330957,
                                 0.        ,1,0.        ,  0.        ,
                                 -0.25881905,0,0.96592583,-61.72834225,
                                 0.        ,0,0.        ,  1;

    left_elbow_correct << 0.994255,-0.107038,0,
                          0.107038,0.994255,0,
                          0,0,1;
    
    right_elbow_correct << 0.994255,0.107038,0,
                           -0.107038,0.994255,0,
                           0,0,1;
}

float hand_len = 115.8953121;
float arm_1_len = 231.256589;
float arm_2_len = 226.130272;
float elbow_theta = 2.924593;

float get_cost(Eigen::VectorXd joints, Eigen::VectorXd last_joints)
{
    Eigen::VectorXd delta_joint(7);
    delta_joint = joints - last_joints;
    return fabs(2*delta_joint(0))+fabs(delta_joint(1))+2*fabs(delta_joint(2));
}

bool leftArmInverse(Eigen::Vector3d position, Eigen::Vector3d orientation, Eigen::VectorXd &current_joints, bool hand_flag)
{
    float min_cost = 0;
    Eigen::VectorXd final_joints(7);
    bool find_result = false;
    Eigen::Vector4d hand_target, target;
    hand_target << position(0), position(1), position(2), 1;
    Eigen::Matrix4d hand_1, hand_2, hand_3;
    Eigen::Vector4d hand;
    hand << 0, 0, hand_len, 1;
    hand_1 << cos(orientation(0)),-sin(orientation(0)),0,0,
              sin(orientation(0)),cos(orientation(0)),0,0,
              0,0,1,0,
              0,0,0,1;
    hand_2 << 1,0,0,0,
              0,cos(orientation(1)),sin(orientation(1)),0,
              0,-sin(orientation(1)),cos(orientation(1)),0,
              0,0,0,1;
    hand_3 << cos(orientation(2)),0,-sin(orientation(2)),0,
              0,1,0,0,
              sin(orientation(2)),0,cos(orientation(2)),0,
              0,0,0,1;
    hand = hand_1 * hand_3 * hand_2 * hand;
    // 求得腕关节在基坐标系的坐标，用于求解前4个关节角度
    target = hand_target - hand;
    target = hand_target;
    target(3) = 1;
    // 将目标点的坐标转换至手臂肩部基坐标系
    hand_target = robotBase_to_leftArmBase * hand_target;
    target = robotBase_to_leftArmBase * target;
    left_last_joints = current_joints;
    float distance = sqrt(target(0)*target(0) + target(1)*target(1) + target(2)*target(2));
    // 判断目标点是否在活动范围内
    if(distance > arm_1_len + arm_2_len)
    {
        current_joints = left_last_joints;
        std::cout << "left distance is out of range" << std::endl;
        std::cout << position << std::endl;
        return false;
    }
    // 根据余弦定理求解肘部关节
    current_joints(3) = acos((arm_1_len*arm_1_len + arm_2_len*arm_2_len - distance*distance)/(2*arm_1_len*arm_2_len));
    current_joints(3) = elbow_theta + current_joints(3) - 2*M_PI;
    if(current_joints(3) < -2.357 || current_joints(3) > 0)
    {
        current_joints = left_last_joints;
        std::cout << "left joints(3) is out of range" << std::endl;
        std::cout << position << std::endl;
        return false;
    }
    float tmp_alpha = atan2(target(1), target(2));
    float tmp_beta = -atan2(target(0), sqrt(target(1)*target(1) + target(2)*target(2)));
    float h = sqrt(arm_1_len*arm_1_len - distance*distance/4);
    Eigen::Vector3d error_elbow;
    error_elbow << distance/2, h, 0;
    error_elbow = left_elbow_correct * error_elbow;
    h = error_elbow(1);
    distance = error_elbow(0) * 2;
    for(float i = -2.007; i <= 2.007; i+=0.017)
    {
        float tmp_x = h * cos(i);
        float tmp_y = h * sin(i);
        float tmp_z = distance / 2;
        Eigen::Matrix3d matrix_1, matrix_2;
        matrix_1 << 1,0,0,
                    0,cos(tmp_alpha),sin(tmp_alpha),
                    0,-sin(tmp_alpha),cos(tmp_alpha);
        matrix_2 << cos(tmp_beta),0,-sin(tmp_beta),
                    0,1,0,
                    sin(tmp_beta),0,cos(tmp_beta);
        Eigen::Vector3d point;
        point << tmp_x, tmp_y, tmp_z;
        // 计算肘部在基坐标系中的坐标
        point = matrix_1 * matrix_2 * point;
        // 计算机械臂关节1和关节2的角度
        current_joints(0) = atan2(point(1), point(2));
        if(current_joints(0) < -0.872 || current_joints(0) > 3.14)
        {
            continue;
        }
        current_joints(1) = -atan2(point(0), sqrt(point(1)*point(1) + point(2)*point(2)));
        if(current_joints(1) < -1.657 || current_joints(1) > 0.105)
        {
            continue;
        }
        Eigen::Matrix4d shoulder_1, shoulder_2;
        shoulder_1 << 1,0,0,0,
                    0,cos(current_joints(0)),sin(current_joints(0)),0,
                    0,-sin(current_joints(0)),cos(current_joints(0)),0,
                    0,0,0,1;
        shoulder_2 << cos(current_joints(1)),0,-sin(current_joints(1)),0,
                    0,1,0,0,
                    sin(current_joints(1)),0,cos(current_joints(1)),0,
                    0,0,0,1;
        // 计算末端坐标在肘部坐标系中的投影坐标
        Eigen::Vector4d tmp_target = shoulder_2.transpose() * shoulder_1.transpose() * target;
        // 计算机械臂关节3的角度
        current_joints(2) = atan2(-tmp_target(1), -tmp_target(0));
        if(current_joints(2) > 2.007 || current_joints(2) < -2.007)
        {
            continue;
        }
        // 以上内容是正确的，不能轻易修改！！！
        if(hand_flag == true)
        {
            Eigen::Matrix4d shoulder_3, elbow;
            shoulder_3 << cos(current_joints(2)), -sin(current_joints(2)), 0, 0,
                          sin(current_joints(2)), cos(current_joints(2)), 0, 0,
                          0,0,1,0,
                          0,0,0,1;
            elbow << cos(current_joints(3)),0,sin(current_joints(3)),0,
                     0,1,0,0,
                     -sin(current_joints(3)),0,cos(current_joints(3)),0,
                     0,0,0,1;
            Eigen::Matrix4d original_hand_orientation = hand_1 * hand_3 * hand_2;
            // 计算手掌向量在腕部坐标系的坐标值
            Eigen::Matrix4d relative_hand_orientation = elbow.transpose() * shoulder_3.transpose() * shoulder_2.transpose() * shoulder_1.transpose() * robotBase_to_leftArmBase * original_hand_orientation;
            current_joints(4) = atan2(-relative_hand_orientation(0,1), relative_hand_orientation(1,1));
            if(current_joints(4) > 2.447 || current_joints(4) < -2.447)
            {
                continue;
            }
            current_joints(5) = atan2(-relative_hand_orientation(2,1), sqrt(relative_hand_orientation(0,1)*relative_hand_orientation(0,1) + relative_hand_orientation(1,1)*relative_hand_orientation(1,1)));
            if(current_joints(5) > 0.872 || current_joints(5) < -0.872)
            {
                continue;
            }
            current_joints(6) = atan2(relative_hand_orientation(2,0), relative_hand_orientation(2,2));
            if(current_joints(6) > 0.872 || current_joints(6) < -0.872)
            {
                continue;
            }
        }
        float current_cost = get_cost(current_joints, left_last_joints);
        if(find_result == false || current_cost < min_cost || current_cost == 0)
        {
            find_result = true;
            min_cost = current_cost;
            final_joints = current_joints;
        }
    }
    if(find_result == false)
    {
        current_joints = left_last_joints;
        std::cout << "Left Find No Result" << std::endl;
        std::cout << position << std::endl;
        return false;
    }
    current_joints = final_joints;
    return true;
}

bool rightArmInverse(Eigen::Vector3d position, Eigen::Vector3d orientation, Eigen::VectorXd &current_joints, bool hand_flag)
{
    float min_cost = 0;
    Eigen::VectorXd final_joints(7);
    bool find_result = false;
    Eigen::Vector4d hand_target, target;
    hand_target << position(0), position(1), position(2), 1;
    Eigen::Matrix4d hand_1, hand_2, hand_3;
    Eigen::Vector4d hand;
    hand << 0, 0, hand_len, 1;
    hand_1 << cos(orientation(0)),-sin(orientation(0)),0,0,
              sin(orientation(0)),cos(orientation(0)),0,0,
              0,0,1,0,
              0,0,0,1;
    hand_2 << 1,0,0,0,
              0,cos(orientation(1)),sin(orientation(1)),0,
              0,-sin(orientation(1)),cos(orientation(1)),0,
              0,0,0,1;
    hand_3 << cos(orientation(2)),0,-sin(orientation(2)),0,
              0,1,0,0,
              sin(orientation(2)),0,cos(orientation(2)),0,
              0,0,0,1;
    hand = hand_1 * hand_3 * hand_2 * hand;
    // 求得腕关节在基坐标系的坐标，用于求解前4个关节角度
    target = hand_target - hand;
    target = hand_target;
    target(3) = 1;
    // 将目标点的坐标转换至手臂肩部基坐标系
    hand_target = robotBase_to_rightArmBase * hand_target;
    target = robotBase_to_rightArmBase * target;
    right_last_joints = current_joints;
    float distance = sqrt(target(0)*target(0) + target(1)*target(1) + target(2)*target(2));
    // 判断目标点是否在活动范围内
    if(distance > arm_1_len + arm_2_len)
    {
        current_joints = right_last_joints;
        std::cout << "right distance is out of range" << std::endl;
        std::cout << position << std::endl;
        return false;
    }
    // 根据余弦定理求解肘部关节
    current_joints(3) = acos((arm_1_len*arm_1_len + arm_2_len*arm_2_len - distance*distance)/(2*arm_1_len*arm_2_len));
    current_joints(3) = elbow_theta + current_joints(3) - 2*M_PI;
    if(current_joints(3) < -2.357 || current_joints(3) > 0)
    {
        current_joints = right_last_joints;
        std::cout << "right joints(3) is out of range" << std::endl;
        std::cout << position << std::endl;
        return false;
    }
    float tmp_alpha = atan2(-target(1), target(2));
    float tmp_beta = atan2(target(0), sqrt(target(1)*target(1) + target(2)*target(2)));
    float h = sqrt(arm_1_len*arm_1_len - distance*distance/4);
    Eigen::Vector3d error_elbow;
    error_elbow << distance/2, h, 0;
    error_elbow = left_elbow_correct * error_elbow;
    h = error_elbow(1);
    distance = error_elbow(0) * 2;
    for(float i = -2.007; i <= 2.007; i+=0.017)
    {
        float tmp_x = -h * cos(i);
        float tmp_y = -h * sin(i);
        float tmp_z = distance / 2;
        Eigen::Matrix3d matrix_1, matrix_2;
        matrix_1 << 1,0,0,
                    0,cos(tmp_alpha),-sin(tmp_alpha),
                    0,sin(tmp_alpha),cos(tmp_alpha);
        matrix_2 << cos(tmp_beta),0,sin(tmp_beta),
                    0,1,0,
                    -sin(tmp_beta),0,cos(tmp_beta);
        Eigen::Vector3d point;
        point << tmp_x, tmp_y, tmp_z;
        // 计算肘部在基坐标系中的坐标
        point = matrix_1 * matrix_2 * point;
        // 计算机械臂关节1和关节2的角度
        current_joints(0) = atan2(-point(1), point(2));
        if(current_joints(0) > 0.872 || current_joints(0) < -3.14)
        {
            continue;
        }
        current_joints(1) = atan2(point(0), sqrt(point(1)*point(1) + point(2)*point(2)));
        if(current_joints(1) < -1.657 || current_joints(1) > 0.105)
        {
            continue;
        }
        Eigen::Matrix4d shoulder_1, shoulder_2;
        shoulder_1 << 1,0,0,0,
                    0,cos(current_joints(0)),-sin(current_joints(0)),0,
                    0,sin(current_joints(0)),cos(current_joints(0)),0,
                    0,0,0,1;
        shoulder_2 << cos(current_joints(1)),0,sin(current_joints(1)),0,
                    0,1,0,0,
                    -sin(current_joints(1)),0,cos(current_joints(1)),0,
                    0,0,0,1;
        // 计算末端坐标在肘部坐标系中的投影坐标
        Eigen::Vector4d tmp_target = shoulder_2.transpose() * shoulder_1.transpose() * target;
        // 计算机械臂关节3的角度
        current_joints(2) = atan2(tmp_target(1), tmp_target(0));
        if(current_joints(2) > 2.007 || current_joints(2) < -2.007)
        {
            continue;
        }
        // 以上内容是正确的，不能轻易修改！！！
        if(hand_flag == true)
        {
            Eigen::Matrix4d shoulder_3, elbow;
            shoulder_3 << cos(current_joints(2)), -sin(current_joints(2)), 0, 0,
                          sin(current_joints(2)), cos(current_joints(2)), 0, 0,
                          0,0,1,0,
                          0,0,0,1;
            elbow << cos(current_joints(3)),0,-sin(current_joints(3)),0,
                     0,1,0,0,
                     sin(current_joints(3)),0,cos(current_joints(3)),0,
                     0,0,0,1;
            Eigen::Matrix4d original_hand_orientation = hand_1 * hand_3 * hand_2;
            // 计算手掌向量在腕部坐标系的坐标值
            Eigen::Matrix4d relative_hand_orientation = elbow.transpose() * shoulder_3.transpose() * shoulder_2.transpose() * shoulder_1.transpose() * robotBase_to_rightArmBase * original_hand_orientation;
            current_joints(4) = atan2(-relative_hand_orientation(0,1), relative_hand_orientation(1,1));
            if(current_joints(4) > 2.447 || current_joints(4) < -2.447)
            {
                continue;
            }
            current_joints(5) = atan2(relative_hand_orientation(2,1), sqrt(relative_hand_orientation(0,1)*relative_hand_orientation(0,1) + relative_hand_orientation(1,1)*relative_hand_orientation(1,1)));
            if(current_joints(5) > 0.872 || current_joints(5) < -0.872)
            {
                continue;
            }
            current_joints(6) = atan2(-relative_hand_orientation(2,0), relative_hand_orientation(2,2));
            if(current_joints(6) > 0.872 || current_joints(6) < -0.872)
            {
                continue;
            }
        }
        float current_cost = get_cost(current_joints, right_last_joints);
        if(find_result == false || current_cost < min_cost || current_cost == 0)
        {
            find_result = true;
            min_cost = current_cost;
            final_joints = current_joints;
        }
    }
    if(find_result == false)
    {
        current_joints = right_last_joints;
        std::cout << "Right Find No Result" << std::endl;
        std::cout << position << std::endl;
        return false;
    }
    current_joints = final_joints;
    return true;
}